/*
 2011 (C) Stefan Grufman
 MyPilot

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 any later version. see <http://www.gnu.org/licenses/>
 */

#include "defines.h"
#include <FastSerial.h>
#include "../mavlink/include/mavlink.h"        // Mavlink interface


FastSerialPort(Serial, 0);


// Define the system type (see mavlink_types.h for list of possible types)
const int system_type = MAV_QUADROTOR;
const int autopilot_type = MAV_AUTOPILOT_GENERIC;

// Initialize the required buffers
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];

unsigned int parameter_i = PARAM_COUNT;

void mavlinkcom_setup()
{
	Serial.begin(115200);
}

void mavlinkcom_loop(unsigned long current_ms)
{
	// Pack the message
	// mavlink_message_heartbeat_pack(system id, component id, message container, system type, MAV_AUTOPILOT_GENERIC)
	mavlink_msg_heartbeat_pack(params_get(PARAM_SYSTEM_ID), params_get(PARAM_COMPONENT_ID), &msg, system_type, autopilot_type);

	// Copy the message to send buffer
	uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

	// Send the message (.write sends as bytes)
	Serial.write(buf, len);

	// Check if we have got some stuff for us?
	mavlinkcom_receive();

	// Sending params?
	parameter_i = mavlinkcom_send_one_parameter(parameter_i);
}

void mavlinkcom_receive()
{
	mavlink_message_t msg;
	mavlink_status_t status;

	//receive data over serial
	while (Serial.available() > 0)
	{
		uint8_t c = Serial.read();

		//try to get a new message
		if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status))
		{
			// Handle message
			switch (msg.msgid)
			{
			case MAVLINK_MSG_ID_SET_MODE:
				// set mode
				break;
			case MAVLINK_MSG_ID_ACTION:
				// EXECUTE ACTION
				break;
			case MAVLINK_MSG_ID_PARAM_SET:
				// Receive a new value of a parameter
				mavlink_param_set_t set;
				mavlink_msg_param_set_decode(&msg, &set);
				mavlinkcom_receive_parameter(&set);
				break;
			case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
				// Start sending parameters
				parameter_i = 0;
				break;
			default:
				//Do nothing
				break;
			}
		}
		// And get the next one
	}
}

static void mavlinkcom_receive_parameter(mavlink_param_set_t* set)
{
	// Search for the parameter
	unsigned int param_id = params_find_param((char*)set->param_id);
	if (param_id != PARAM_COUNT)
	{
		params_set(param_id, set->param_value);

		// Now, we also need to send it...
		mavlinkcom_send_one_parameter(param_id);
	}
}

unsigned int mavlinkcom_send_one_parameter(unsigned int p_id)
{
	//  send parameters one by one
	if (p_id < PARAM_COUNT)
	{
		// pack it
		mavlink_msg_param_value_pack(params_get(PARAM_SYSTEM_ID), params_get(PARAM_COMPONENT_ID), &msg,
				(int8_t*)params_get_name(p_id), params_get(p_id), PARAM_COUNT, p_id);

		// Copy the message to send buffer
		uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

		// Send the message (.write sends as bytes)
		Serial.write(buf, len);

		return (p_id + 1);
	}
	else
	{
		return PARAM_COUNT;
	}
}
